/**
  ******************************************************************************
  * @file           : QMI8658.cpp
  * @author         : admin
  * @brief          : None
  * @attention      : None
  * @date           : 2024/10/11
  ******************************************************************************
  */

#include "QMI8658.h"
static uint8_t i2cread(void)
{
    return Wire.read();
}

static void i2cwrite(uint8_t x)
{
    Wire.write((uint8_t)x);
}

static void writeRegister(uint8_t i2cAddress, uint8_t reg, uint8_t value)
{
    Wire.setClock(400000);
    Wire.beginTransmission(i2cAddress);
    i2cwrite(reg);
    i2cwrite(value);
    Wire.endTransmission();
}
static int16_t readRegister(uint8_t i2cAddress, uint8_t reg)
{
    Wire.setClock(400000);
    Wire.beginTransmission(i2cAddress);
    i2cwrite((uint8_t)reg);
    Wire.endTransmission();
    Wire.requestFrom(i2cAddress, (uint8_t)2);
    return (int16_t)(i2cread() | (i2cread() << 8));
}

static void _readReg(uint8_t *buf, uint8_t reg, uint8_t len)
{
    Wire.setClock(400000);
    Wire.beginTransmission(106);
    i2cwrite((uint8_t)reg);
    Wire.endTransmission();
    Wire.requestFrom(106, (uint8_t)len);
    for (uint8_t i = 0; i < len; i++)
    {
        buf[i] = i2cread();
    }
}

QMI8658::QMI8658()
{
    for (uint8_t i = 0; i < 7; i++)
    {
        GestureHandle[i] = NULL;
    }
    offsetX = 0;
    offsetY = 0;
    offsetZ = 0;
}

void QMI8658::init(void)
{
    Wire.begin();
    Wire.beginTransmission(106);
    if (Wire.endTransmission() == 0)
    {
        chip = 2;
        writeRegister(QMI8658_I2C_ADDR, 0x60, 0x01);
        delayMicroseconds(20);
        writeRegister(QMI8658_I2C_ADDR, 0x02, 0x60);
        writeRegister(QMI8658_I2C_ADDR, 0x08, 0x03);
        writeRegister(QMI8658_I2C_ADDR, 0x03, 0x1c);
        writeRegister(QMI8658_I2C_ADDR, 0x04, 0x40);
        writeRegister(QMI8658_I2C_ADDR, 0x06, 0x55);

        uint16_t id = readRegister(QMI8658_I2C_ADDR, 0x0);
        uint8_t format[1] = {0};
        _readReg(format, 0x03, 1);
        Serial.println(format[0]);
        format[0] = format[0] & 0x8f;
        format[0] |= 0;
        Serial.println(format[0]);
        writeRegister(QMI8658_I2C_ADDR, 0x03, format[0]);

    }
    else
    {
        chip = 1;
        writeRegister(MSA300_I2C_ADDR, 0x0F, 0x08);
        writeRegister(MSA300_I2C_ADDR, 0x11, 0x00);
    }

    delay(100);
}

float QMI8658::getRawX(void)
{
    if (chip == 1)
    {
        uint16_t v = 0, retry = 3;
        do
        {
            v = readRegister(MSA300_I2C_ADDR, 0x02);
            if (v == 0xffff)
            {
                init();
            }
        } while ((v == 0xffff) && (--retry));
        rawX = ((int16_t)v) * 1.0 / 4 / 4096;
        return rawX;
    }
    else
    {
        int16_t x = 0;
        uint8_t buffer[2] = {0};
        _readReg(buffer, 0x35, 2);
        x = (buffer[1] << 8) | buffer[0];
        rawX = (x * 2) / 32768.00000;
        return rawX;
    }
}

float QMI8658::getRawY(void)
{
    if (chip == 1)
    {
        uint16_t v = 0, retry = 3;
        do
        {
            v = readRegister(MSA300_I2C_ADDR, 0x04);
            if (v == 0xffff)
            {
                init();
            }
        } while ((v == 0xffff) && (--retry));
        rawY = ((int16_t)v) * 1.0 / 4 / 4096;
        return rawY;
    }
    else
    {
        int16_t y = 0;
        uint8_t buffer[2] = {0};
        _readReg(buffer, 0x37, 2);
        y = (buffer[1] << 8) | buffer[0];
        rawY = (y * 2) / 32768.00000;
        return rawY;
    }
}

float QMI8658::getRawZ(void)
{
    if (chip == 1)
    {
        uint16_t v = 0, retry = 3;
        do
        {
            v = readRegister(MSA300_I2C_ADDR, 0x06);
            if (v == 0xffff)
            {
                init();
            }
        } while ((v == 0xffff) && (--retry));
        rawZ = ((int16_t)v) * 1.0 / 4 / 4096;
        return rawZ;
    }
    else
    {
        int16_t z = 0;
        uint8_t buffer[2] = {0};
        _readReg(buffer, 0x39, 2);
        z = (buffer[1] << 8) | buffer[0];
        rawZ = (z * 2) / 32768.00000;
        return rawZ;
    }
}

float QMI8658::getX(void)
{
    if (!isGestureEnable)
    {
        return (chip == 1) ? (getRawX() * 1000) : (getRawX() * 1000 + offsetX);
        // return getRawX() * 1000; // g to mg
    }
    return rawX * 1000;
}

float QMI8658::getY(void)
{
    if (!isGestureEnable)
    {
        return (chip == 1) ? (getRawY() * 1000) : (getRawY() * 1000 + offsetY);
        // return getRawY() * 1000; // g to mg
    }
    return rawY * 1000;
}

float QMI8658::getZ(void)
{
    if (!isGestureEnable)
    {
        return (chip == 1) ? (getRawZ() * 1000) : (getRawZ() * 1000 + offsetZ);
        // return getRawZ() * 1000; // g to mg
    }
    return rawZ * 1000;
}

void QMI8658::setOffset(int x, int y, int z)
{
    offsetX = x;
    offsetY = y;
    offsetZ = z;
}

float QMI8658::getStrength(void)
{
    float x = getX();
    float y = getY();
    float z = getZ();
    return sqrt(x * x + y * y + z * z);
}

void QMI8658::onGesture(Gesture gesture, mpythonGestureHandlePtr body)
{
    if (!isGestureEnable)
    {
        xTaskCreatePinnedToCore(QMI8658::taskLoop, "gestureTask", 2048, this, 1, NULL, ARDUINO_RUNNING_CORE);
        isGestureEnable = true;
    }
    GestureHandle[gesture] = body;
}

bool QMI8658::isGesture(Gesture gesture)
{
    if (!isGestureEnable)
    {
        xTaskCreatePinnedToCore(QMI8658::taskLoop, "gestureTask", 2048, this, 1, NULL, ARDUINO_RUNNING_CORE);
        isGestureEnable = true;
    }
    if (gesture != currentGesture)
        return false;
    return true;
}
QMI8658 accelerometer;